/**
 * Copyright (C) BFH www.bfh.ch 2011
 * Code written by: Patrick Dobler, Marc Folly
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package ch.bfh.ti.kybernetik.engine.controller;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Iterator;
import java.util.List;
import java.util.Observable;
import java.util.concurrent.CopyOnWriteArrayList;

import javax.vecmath.Point2d;
import javax.vecmath.Vector2d;

import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.ArrayRealVector;
import org.apache.commons.math.linear.DecompositionSolver;
import org.apache.commons.math.linear.InvalidMatrixException;
import org.apache.commons.math.linear.LUDecompositionImpl;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import ch.bfh.ti.kybernetik.engine.controller.lightBulb.LightBulbController;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.SimulatorObserverCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightBulbModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightBulbModelObserverCommand.LightBulbModelCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightSensorModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightSensorModelObserverCommand.LightSensorCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterModelObserverCommand.RoboterModelCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterMoveListModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterMoveListModelObserverCommand.RoboterMoveModelObserverCommandState;
import ch.bfh.ti.kybernetik.engine.controller.roboter.RoboterConstruction;
import ch.bfh.ti.kybernetik.engine.controller.roboter.RoboterController;
import ch.bfh.ti.kybernetik.engine.model.LightBulb;
import ch.bfh.ti.kybernetik.engine.model.Roboter;
import ch.bfh.ti.kybernetik.engine.model.RoboterMove;
import ch.bfh.ti.kybernetik.lego.ki.RobKIFactr;
import ch.bfh.ti.kybernetik.lego.ki.RoboterKI;

/**
 * The default {@link Simulator} implementation
 * 
 */
class DefaultSimulatorImpl extends Observable implements Simulator {
	private static Logger logger = LoggerFactory.getLogger(DefaultSimulatorImpl.class);

	private final List<LightBulbController> lightBulbControllers;

	private final List<RoboterController> roboterControllers;

	private static int height = 10;

	private static int width = 10;

	private volatile boolean simulating = false;

	private volatile boolean running = true;

	private volatile int simulatorSpeed = 10;

	public DefaultSimulatorImpl(LightBulbController[] lightBulbController, RoboterController... roboterController) {
		this.roboterControllers = new CopyOnWriteArrayList<RoboterController>(roboterController);
		this.lightBulbControllers = new CopyOnWriteArrayList<LightBulbController>(lightBulbController);
	}

	@Override
	public void run() {

		this.notifyObservers(SimulatorObserverCommandState.STARTED_SIMULATION);

		while (running) {

			Thread.yield();

			while (simulating) {
				try {

					Thread.sleep(simulatorSpeed);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				this.notifyObservers(SimulatorObserverCommandState.START_ROBOTER_MOVING);

				this.iterateRoboterControllers(new ThreadSafeIterationAction<RoboterController>() {
					@Override
					public void performAction(RoboterController roboterController) {
						// System.out.println("Simulator: Iterate RoboterController");
						double leftSensorIntensity = 0;
						double rightSensorIntensity = 0;
						Roboter roboter = roboterController.getRoboter();
						RoboterConstruction roboterConstruction = roboterController.getRoboterConstruction();
						for (LightBulbController lightBulbController : lightBulbControllers) {

							// Left Light Sensor
							Point2d leftLightSensorPoint = roboterConstruction.getRoboterElementPoint(roboter.getLeftLightSensor());
							Vector2d leftLightSensorVector = roboterConstruction.getRoboterElementAngleVector(roboter.getLeftLightSensor());

							leftSensorIntensity = leftSensorIntensity
									+ lightBulbController.calculateIntensity(leftLightSensorPoint, roboter.getLeftLightSensor(),
											leftLightSensorVector);

							// Right Light Sensor
							Point2d rightLightSensorPoint = roboterConstruction.getRoboterElementPoint(roboter.getRightLightSensor());
							Vector2d rightLightSensorVector = roboterConstruction.getRoboterElementAngleVector(roboter
									.getRightLightSensor());

							rightSensorIntensity = rightSensorIntensity
									+ lightBulbController.calculateIntensity(rightLightSensorPoint, roboter.getRightLightSensor(),
											rightLightSensorVector);

						}
						// Changed Left Sensor Intensity
						if (leftSensorIntensity != roboter.getLeftLightSensor().getSensorIntensity()) {
							notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_CHANGED,
									roboter.getLeftLightSensor()));
							logger.debug("Changed Left Light Sensor Intensity to {}: " + leftSensorIntensity);
							roboter.getLeftLightSensor().setSensorIntensity(leftSensorIntensity);
						}
						if (leftSensorIntensity == 0) {
							notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_REMAINS,
									roboter.getLeftLightSensor()));
						}
						// Changed Right Sensor Intensity
						if (rightSensorIntensity != roboter.getRightLightSensor().getSensorIntensity()) {
							notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_CHANGED,
									roboter.getRightLightSensor()));
							logger.debug("Changed Right Light Sensor Intensity to {}: " + rightSensorIntensity);
							roboter.getRightLightSensor().setSensorIntensity(rightSensorIntensity);
						}
						if (rightSensorIntensity == 0) {
							notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_REMAINS,
									roboter.getRightLightSensor()));
						}
						RoboterMove rm = roboterController.calculateNewRoboterPosition(leftSensorIntensity, rightSensorIntensity);

						calculateOutOfWindowAreaPosition(roboterController, rm);

						notifyObservers(new RoboterMoveListModelObserverCommand(roboter,
								RoboterMoveModelObserverCommandState.ROBOTER_MOVE_ADDED, roboterController.getRoboterMoveHistory()));
						notifyObservers(new RoboterModelObserverCommand(roboter,
								RoboterModelObserverCommand.RoboterModelCommandState.ROBOTER_MOVED));
					}

				});
				this.notifyObservers(SimulatorObserverCommandState.MOVED_ALL_ROBOTOS);
			}
			this.notifyObservers(SimulatorObserverCommandState.PAUSED_SIMULATING);
		}
		this.notifyObservers(SimulatorObserverCommandState.STOPPED_SIMULATION);

	}

	private void calculateOutOfWindowAreaPosition(RoboterController roboterController, RoboterMove rm) {
		Point2d newPosition = null;

		// Fall 1: Unten
		Point2d roboter1 = rm.getNewRoboterPoint();
		if (roboter1.getY() > height) {
			newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_LEFT, SideStraigtLine.LINE_RIGHT,
					SideStraigtLine.LINE_UP);
			roboterController.getRoboter().setX(newPosition.getX());
			roboterController.getRoboter().setY(newPosition.getY());

			// Fall 2: Oben
		} else if (roboter1.getY() < 0) {
			newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_LEFT, SideStraigtLine.LINE_RIGHT,
					SideStraigtLine.LINE_DOWN);
			roboterController.getRoboter().setX(newPosition.getX());
			roboterController.getRoboter().setY(newPosition.getY());

			// Fall 3: Rechts
		} else if (roboter1.getX() > width) {
			newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_LEFT, SideStraigtLine.LINE_DOWN,
					SideStraigtLine.LINE_UP);
			roboterController.getRoboter().setX(newPosition.getX());
			roboterController.getRoboter().setY(newPosition.getY());

			// Fall 4: Links
		} else if (roboter1.getX() < 0) {
			newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_RIGHT, SideStraigtLine.LINE_DOWN,
					SideStraigtLine.LINE_UP);
			roboterController.getRoboter().setX(newPosition.getX());
			roboterController.getRoboter().setY(newPosition.getY());

		}
	}

	public boolean isPointWithinField(Point2d p1) {
		int t = (int) p1.getX();
		if ((int) p1.getX() >= 0 && (int) p1.getX() <= width) {
			if ((int) p1.getY() >= 0 && (int) p1.getY() <= height) {
				return true;
			}
		}
		return false;
	}

	private Point2d getIntersectionPointForSides(RoboterMove rm, Point2d roboter1, SideStraigtLine... lines) {
		for (SideStraigtLine line : lines) {
			try {
				Point2d p1 = getIntersectionPointForSide(rm, roboter1, line);
				if (isPointWithinField(p1)) {
					// System.out.println("Found Intersection on Side: " +
					// line);
					return p1;
				} else {
					// System.out.println("No Intersection   Found on Side:   "
					// + line);
				}
			} catch (InvalidMatrixException ime) {
				// System.out.println("Line " + line + " was parallel");
			}
		}
		return null;
	}

	private Point2d getIntersectionPointForSide(RoboterMove rm, Point2d roboter1, SideStraigtLine line) throws InvalidMatrixException {
		double sideX = line.sideX;
		double sideY = line.sideY;
		double sideVectX = line.sideVectX;
		double sideVectY = line.sideVectY;

		Vector2d direction = rm.getNewDirectionVector();
		direction.normalize();
		RealMatrix coefficients = new Array2DRowRealMatrix(new double[][] { { direction.getX(), -(sideVectX) },
				{ direction.getY(), -(sideVectY) } }, false);
		DecompositionSolver solver = new LUDecompositionImpl(coefficients).getSolver();
		RealVector constants = new ArrayRealVector(new double[] { sideX - roboter1.getX(), sideY - roboter1.getY() }, false);
		RealVector solution = solver.solve(constants);

		double r = solution.getEntry(0);
		// double s = solution.getEntry(1);

		double res1 = roboter1.getX() + direction.getX() * r;
		double res2 = roboter1.getY() + direction.getY() * r;

		Point2d p = new Point2d(res1, res2);
		return p;
	}

	/**
	 * We need to override this method to make sure both calls the setChanged()
	 * and the notifyObservers() are called immediately after each other!
	 * Otherwise another Thread might toggle the changed attribute of the
	 * Observer and the next notifyCommand won't be distributed
	 */
	@Override
	public synchronized void notifyObservers(Object arg) {
		super.setChanged();
		super.notifyObservers(arg);
	}

	public boolean isSimulating() {
		return simulating;
	}

	public void setSimulating(boolean simulating) {
		this.simulating = simulating;
	}

	public boolean isRunning() {
		return running;
	}

	public void setRunning(boolean running) {
		this.running = running;
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see ch.bfh.ti.kybernetik.simulator.controller.SimulatorInterface#
	 * getSimulatorSpeed()
	 */
	@Override
	public int getSimulatorSpeed() {
		return simulatorSpeed;
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see ch.bfh.ti.kybernetik.simulator.controller.SimulatorInterface#
	 * setSimulatorSpeed(int)
	 */
	@Override
	public void setSimulatorSpeed(int simulatorSpeed) {
		if (simulatorSpeed >= 0)
			this.simulatorSpeed = simulatorSpeed;
	}

	@Override
	public void setWidth(int width) {
		this.width = width;
	}

	@Override
	public int getWidth() {
		return width;
	}

	@Override
	public void setHeight(int height) {
		this.height = height;
	}

	@Override
	public int getHeight() {
		return height;
	}

	@Override
	public boolean addRoboterController(RoboterController roboterController) {

		boolean added = this.getRoboterControllers().add(roboterController);
		if (added) {
			this.notifyObservers(new RoboterModelObserverCommand(roboterController.getRoboter(), RoboterModelCommandState.ROBOTER_ADDED));
		}
		return added;
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see
	 * ch.bfh.ti.kybernetik.engine.controller.Simulator#addLightBulbController
	 * (ch.bfh.ti.kybernetik.engine.controller.lightBulb.LightBulbController)
	 */
	@Override
	public boolean addLightBulbController(LightBulbController lightBulbController) {
		boolean added = this.getLightBulbControllers().add(lightBulbController);
		if (added) {
			this.notifyObservers(new LightBulbModelObserverCommand(lightBulbController.getLightBulb(),
					LightBulbModelCommandState.LIGHTBULB_ADDED));
		}
		return added;

	}

	@Override
	public void removeRoboter(Roboter roboter) {
		RoboterController rcToRemove = this.getRoboterControllerForRoboter(roboter);
		if (rcToRemove != null) {
			this.getRoboterControllers().remove(rcToRemove);
			this.notifyObservers(new RoboterModelObserverCommand(rcToRemove.getRoboter(), RoboterModelCommandState.ROBOTER_REMOVED));
		}
	}

	@Override
	public void removeLightBulb(LightBulb lightBulb) {
		LightBulbController lbcToRemove = this.getLightBulbControllerForLightBulb(lightBulb);
		if (lbcToRemove != null) {
			this.getLightBulbControllers().remove(lbcToRemove);
			this.notifyObservers(new LightBulbModelObserverCommand(lbcToRemove.getLightBulb(), LightBulbModelCommandState.LIGHTBULB_REMOVED));
		}
	}

	@Override
	public void iterateRoboterControllers(ThreadSafeIterationAction<RoboterController> action) {
		Collection<RoboterController> list = getRoboterControllers();
		final Iterator<RoboterController> i = list.iterator();
		while (i.hasNext()) {
			action.performAction(i.next());
		}
	}

	@Override
	public List<Roboter> getRoboterList() {
		final List<Roboter> roboterList = new ArrayList<Roboter>();
		this.iterateRoboterControllers(new ThreadSafeIterationAction<RoboterController>() {
			@Override
			public void performAction(RoboterController iterateObject) {
				roboterList.add(iterateObject.getRoboter());

			}
		});
		return roboterList;

	}

	@Override
	public List<LightBulb> getLightBulbList() {
		final List<LightBulb> lightBulbList = new ArrayList<LightBulb>();
		this.iterateLightBulbControllers(new ThreadSafeIterationAction<LightBulbController>() {
			@Override
			public void performAction(LightBulbController iterateObject) {
				lightBulbList.add(iterateObject.getLightBulb());

			}
		});
		return lightBulbList;
	}

	@Override
	public void iterateLightBulbControllers(ThreadSafeIterationAction<LightBulbController> action) {
		Collection<LightBulbController> list = getLightBulbControllers();
		final Iterator<LightBulbController> i = list.iterator();
		while (i.hasNext()) {
			action.performAction(i.next());
		}
	}

	private List<RoboterController> getRoboterControllers() {
		return roboterControllers;
	}

	@Override
	public void setDeltaT(final double deltaT) {
		this.iterateRoboterControllers(new ThreadSafeIterationAction<RoboterController>() {
			@Override
			public void performAction(RoboterController iterateObject) {
				iterateObject.setDeltaT(deltaT);
			}
		});
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see
	 * ch.bfh.ti.kybernetik.simulator.controller.SimulatorInterface#getDeltaT()
	 */
	@Override
	public double getDeltaT() {
		return this.getRoboterControllers().get(0).getDeltaT();
	}

	private List<LightBulbController> getLightBulbControllers() {
		return lightBulbControllers;
	}

	@Override
	public RoboterKI getRoboterKIForRoboter(Roboter roboter) {
		RoboterController roboterController = this.getRoboterControllerForRoboter(roboter);
		if (roboterController != null) {
			return roboterController.getRoboterKI();
		}
		return null;
	}

	@Override
	public RoboterConstruction getRoboterConstructionForRoboter(Roboter roboter) {
		RoboterController roboterController = this.getRoboterControllerForRoboter(roboter);
		if (roboterController != null) {
			return roboterController.getRoboterConstruction();
		}
		return null;
	}

	private RoboterController getRoboterControllerForRoboter(Roboter roboter) {
		Collection<RoboterController> list = getRoboterControllers();
		final Iterator<RoboterController> i = list.iterator();
		while (i.hasNext()) {
			RoboterController roboterController = i.next();
			if (roboterController.getRoboter().equals(roboter)) {
				return roboterController;
			}
		}
		return null;
	}

	private LightBulbController getLightBulbControllerForLightBulb(LightBulb lightBulb) {
		Collection<LightBulbController> list = getLightBulbControllers();
		final Iterator<LightBulbController> i = list.iterator();
		while (i.hasNext()) {
			LightBulbController lightBulbController = i.next();
			if (lightBulbController.getLightBulb().equals(lightBulb)) {
				return lightBulbController;
			}
		}
		return null;
	}

	@Override
	public void changeRoboterKIForRoboter(Roboter roboter, RoboterKI roboterKI) {
		RoboterController roboterController = this.getRoboterControllerForRoboter(roboter);
		roboterController.setRoboterKI(roboterKI);
	}

	private enum SideStraigtLine {
		LINE_LEFT(0, 0, 0, 1), LINE_RIGHT(width, 0, 0, 1), LINE_DOWN(0, height, 1, 0), LINE_UP(0, 0, 1, 0);

		double sideX = 0;
		double sideY = 0;
		double sideVectX = 0;
		double sideVectY = 1;

		SideStraigtLine(double sideX, double sideY, double sideVectX, double sideVectY) {
			this.sideVectX = sideVectX;
			this.sideVectY = sideVectY;
			this.sideX = sideX;
			this.sideY = sideY;
		}

	}

}
